/*****************************************************************//**
 * \file   RobotControlInterface.h
 * \brief  所有机械控制接口基类
 * 
 * \author Administrator
 * \date   August 2021
 * \modify by Administrator
 * \date   August 2021

 *********************************************************************/
#pragma once

#include "Eigen/Dense"

#define NOARM			0  
#define ARMTYPE_AOBOi5	1
#define ARMTYPE_UR3		2
#define ARMTYPE_FRANKA	3

#define AuboRobotNetPort  8899
#define FrankaRobotNetPort 9999
#define UR3RobotNetPort 30003//9988

//机械臂连接结构体
struct sRobotArmCtrl
{
	//机械臂是否连接
	bool bRobotArmConnect;
};

//机械臂实时动态数据结构体
struct sRobotDyParameterCtrl
{
	//机械臂自由度
	int iFreedom;

	//机械臂实时各关节角（弧度）
	double dCoo6FJoint[6];

	//机械臂末端实时位置
	double dPosturePx, dPosturePy, dPosturePz;
	Eigen::Vector3d position;

	//机械臂末端实时姿态
	double dPostureOx, dPostureOy, dPostureOz;
	Eigen::Matrix3d orientation;

	//机械臂末端实时位置与姿态
	Eigen::Matrix4d m4dRobotEnd;

	//机械臂末端所夹持的手术器械位置
	Eigen::Vector3d v3dToolPosture;
	double dToolPx, dToolPy, dToolPz;
};

//电机数据结构体
struct sMotorDeviceCtrl
{
	bool bMotorDeviceConnect[5];
	double dMotorState[5];
	Eigen::Matrix4d m4dToolEnd;
};

/*
  机械臂实时数据结构
*/
struct SRobotRealTimeData {
	sRobotArmCtrl sRobotArm;
	sRobotDyParameterCtrl sRobotDy;
	sMotorDeviceCtrl sMotorDevice;
};

class RobotControlInterface
{
public:
	bool bInitDone = false;
    bool isArmInit = false;
    bool isToolInit = false;
public:
	
	virtual bool robotArmLogin() { return true; };

	virtual bool robotLogin(const char* addr, int port) { return true; };

	virtual void robotForward(Eigen::Matrix4d& EndMatrix, double robot_current_joint[6]) {};

	virtual void robotInverse(Eigen::Matrix3d orientation_matrix, Eigen::Vector3d transposition, double joint_current[6], double* joint_move) {};

	virtual void robotMove(double* pos) {};

	virtual void robotMove(Eigen::Matrix4d desired_EndMatrix) {};

	virtual bool robotMoveJ(double* pos) { return false; };

	virtual void robotMoveJ(Eigen::Matrix4d desired_EndMatrix) {};

	virtual bool robotRemove() { return false; };

	virtual bool robotGetConnectState() { return false; };

	virtual void GetRealtimeState(SRobotRealTimeData& sRunTimedata) { return; };

	virtual void robotGetJointStatus() {};

	virtual bool RobotArm_Shutdown() { return true; };

	virtual bool robotShutdown() { return true; };

	virtual void test_hahaha() { return; };
};

